#include <vector> 

int linear_fit(float*,float*,unsigned short,float&,float&);

/** Calculate the gain and offset of a set of images
 *
 * @param images	Input images
 * @param sizeX		Input image size in X direction
 * @param sizeY		Input image size in y direction
 * @param gainOut	Pointer to array of output gain data. 16 bit ushort format
 * @param offsetOut	Pointer to array of output offset data. 16 bit ushort format, amplified by 256
 * @param bitDepth	Bit depth of the input image
 */
void calculateGainOffset(const std::vector <unsigned short*> & images , int sizeX, int sizeY, unsigned short** gainOut, unsigned short** offsetOut,int bitDepth=16)
{
	unsigned short imageNumber = images.size();
	float* gain = new float[sizeX*sizeY];
	float* offset = new float[sizeX*sizeY];
	float* position = new float[imageNumber];
	float* mean = new float[imageNumber];
	for(int j =0;j<imageNumber;j++)//compute the mean intensity of every image
	{
		mean[j] = 0;
		for(int i=0; i< sizeX*sizeY;i++)
		{
			mean[j] = (float)images[j][i] + mean[j];
		}
		mean[j] = mean[j]/(float)(sizeX*sizeY);
	}

	for(int i=0; i< sizeX*sizeY;i++)//linear fitting for every pixel
	{
		for(int j =0;j<imageNumber;j++)
		{
			position[j] = images[j][i];
		}
		int mark = linear_fit(mean,position,imageNumber,gain[i],offset[i]);
	}
	delete position;
	delete mean;
	*offsetOut = (unsigned short*)malloc(sizeX*sizeY*sizeof(unsigned short));
	*gainOut = (unsigned short*)malloc(sizeX*sizeY*sizeof(unsigned short));
	for(int i = 0;i<sizeX*sizeY;i++)
	{
		unsigned short * data;
		data = &(*offsetOut)[i];
		*data = (unsigned short)(offset[i]);
	}
	for(int i = 0;i<sizeX*sizeY;i++)
	{
		unsigned short * data;
		data = &(*gainOut)[i];
		*data = (unsigned short)(gain[i]*256);//multiply by 256
	}
	delete gain;
	delete offset;
}

/** 1D Linear Fitting with none negative y-intercept
 *
 * @param x			Input X
 * @param y			Input Y
 * @param n			Number of the datas
 * @param gain		Output: the slope
 * @param offset	Output: y-intercept
 */
int linear_fit(float*x,float*y,unsigned short n,float &gain,float &offset)
{
	float fn = (float)n;
	float sumx = 0;
	float sumy = 0;
	float sumx2 = 0;
	float sumxy = 0;
	float a1 = 0;
	float a0; //y = a1*x + a0
	for(int i = 0;i<n;i++)
	{
		sumx = sumx + x[i];
		sumy = sumy + y[i];
		sumx2 = sumx2 + x[i]*x[i];
		sumxy = sumxy + x[i]*y[i];
	}
	a1 = (fn*sumxy - sumx*sumy) / (fn*sumx2 - sumx*sumx);
	a0 = (sumy - a1*sumx)/fn;
	if(a0<0)//y-intercept < 0; fit again with y = a1*x
	{
		a1 = sumxy/sumx2;
		a0 = 0;
		gain = a1;
		offset = a0;
		return 0;
	}
	gain = a1;
	offset = a0;
	return 1;
}